#include "myInit.h"

static inline void myDisableEXTI_NVIC(uint8_t sNum)     //关闭某个外部中断
{
    EXTI->IMR1 &= ~(1 << sNum);        
}

float gShootLastAngle		= 0.f;
float gShootReadAngle       = 0.f;
float gShootNowAngle        = 0.f;
float gShootNowAngle_buff   = 0.f;

uint8_t ClawLRxFlag = 0;
uint8_t ClawRRxFlag = 0;
uint8_t EscaladeLFlag = 0;
uint8_t EscaladeRFlag = 0;

AS5047P_HandleTypeDef shootEnc;
WS2811_HandleTypeDef LEDStrip;

#if ENC_SPEED_ENABLE
volatile float gShootSpeed 	= 0;
#endif

VESC_HandleTypeDef WheelF;     //轮电机
VESC_HandleTypeDef WheelR;
VESC_HandleTypeDef WheelB;
VESC_HandleTypeDef WheelL;
DJI_HandleTypeDef HelmF;        //舵电机
DJI_HandleTypeDef HelmR;
DJI_HandleTypeDef HelmB;
DJI_HandleTypeDef HelmL;

VESC_HandleTypeDef Shoot;     //推射电机

DJI_HandleTypeDef EscaladeL;
DJI_HandleTypeDef EscaladeR;
DJI_HandleTypeDef ClawL;
DJI_HandleTypeDef ClawR;
DJI_HandleTypeDef Defense;

volatile struct myErrState gErrState 			= {0};
volatile bool gMotorRxInitFinishFlag    		= false;
volatile bool gMechInitFinishAndStartEncFlag 	= false;
volatile bool gMechInitFinishAndInFreeRTOSFlag	= false;
volatile bool gMotorONFlag 						= false; 
volatile uint8_t gMechInitMode 					= false; 

uint16_t IDArray[CANNum] = {ShootID,
                            WheelFID, WheelRID, WheelBID, WheelLID,
                            HelmFID, HelmRID, HelmBID, HelmLID,
                            EscaladeLID, EscaladeRID, ClawLID, ClawRID};

uint8_t IDFlagArray[CANNum] = {0};
  
void deviceInit()       //电子元等设备初始化
{
    communicateInit();
    WS2811_Init(LEDStrip,htim3,TIM_CHANNEL_1,20);
    LEDStrip.pf->setColor(&LEDStrip, 1, 20, 255, 255, 255); // 白色, 代表上电
	// 定义CAN发送失败标志位
    
    VESC_Init(&Shoot, &hfdcan2, ShootID, 14, 1);            // 发射本杰明电机初始化

    VESC_Init(&WheelF, &hfdcan1, WheelFID, 7, 1);           // 轮电机初始化
	VESC_Init(&WheelB, &hfdcan1, WheelBID, 7, 1);
    VESC_Init(&WheelR, &hfdcan1, WheelRID, 7, 1);
    VESC_Init(&WheelL, &hfdcan1, WheelLID, 7, 1);
    DJI_Init(&HelmF, &hfdcan1, HelmFID);   // 舵电机初始化
	DJI_Init(&HelmB, &hfdcan1, HelmBID);
    DJI_Init(&HelmR, &hfdcan1, HelmRID);
    DJI_Init(&HelmL, &hfdcan1, HelmLID);

    DJI_Init(&EscaladeL,&hfdcan3,EscaladeLID);              // 运球电机初始化
    DJI_Init(&EscaladeR,&hfdcan3,EscaladeRID);
    DJI_Init(&ClawL, &hfdcan3, ClawLID);
    DJI_Init(&ClawR, &hfdcan3, ClawRID);
    DJI_Init(&Defense, &hfdcan3, DefenseID);

    // 各个电机CAN接收初始化
    VESC_RxInit(VESCNum, (VESC_HandleTypeDef*[]){&Shoot, &WheelF, &WheelR, &WheelB, &WheelL});
    DJI_RxInit(DJINum, (DJI_HandleTypeDef*[]){&EscaladeL, &EscaladeR, &ClawL, &ClawR});
    gMotorRxInitFinishFlag = true;
    
    AS5047P_Init(&shootEnc, &hspi1, GPIOA, GPIO_PIN_4);    	//推射编码器初始化
	
    HAL_TIM_Base_Start_IT(&htim5);  //启动定时器
}


#define cDJIRSTCurrentDutyLimit 35
#define cSetOriginWaitTime 2500 //云梯设置原点等待的时间(ms), 应当设置为云梯从最上端走下来所需最大时间

void mechInit()     //机构初始化(复位等)
{
    while (gMechInitMode == 0)
    {
        HAL_Delay(1);
    }
		
	while (!ClawLRxFlag || !ClawRRxFlag || !EscaladeLFlag || !EscaladeRFlag)
	{
		HAL_Delay(1);
	}
	
	gMotorONFlag = true;
	
    volatile uint32_t sStartTick;
    sStartTick = HAL_GetTick();
	
#if DRIBBLE_ENABLE
    pullCylinder();                                 //气缸拉回初始位置
	
	ClawL.pf->speedMode(&ClawL, -50, cDJIRSTCurrentDutyLimit);
	HAL_Delay(1);
	ClawR.pf->speedMode(&ClawR, 50, cDJIRSTCurrentDutyLimit);
	HAL_Delay(200);
	
    EscaladeL.pf->speedMode(&EscaladeL, -200, cDJIRSTCurrentDutyLimit);   //上电云梯向上动
    HAL_Delay(1);
    EscaladeR.pf->speedMode(&EscaladeR, 200, cDJIRSTCurrentDutyLimit);
    HAL_Delay(1);
    EscaladeL.pf->speedMode(&EscaladeL, -200, cDJIRSTCurrentDutyLimit);   //保险起见发两次
    HAL_Delay(1);
    EscaladeR.pf->speedMode(&EscaladeR, 200, cDJIRSTCurrentDutyLimit);
    HAL_Delay(1);

	while (HAL_GetTick() < sStartTick + cSetOriginWaitTime / 2)
    {
        HAL_Delay(1);
    } 
	
	ClawL.pf->speedMode(&ClawL, 50, cDJIRSTCurrentDutyLimit);
	HAL_Delay(1);
	ClawR.pf->speedMode(&ClawR, -50, cDJIRSTCurrentDutyLimit);
	HAL_Delay(1);
	
    while (HAL_GetTick() < sStartTick + cSetOriginWaitTime)
    {
        HAL_Delay(1);
    } 

    {
        EscaladeL.pf->setOrigin(&EscaladeL);            //云梯电机设置原点
        HAL_Delay(1);
        EscaladeR.pf->setOrigin(&EscaladeR);
        HAL_Delay(1);
        ClawL.pf->setOrigin(&ClawL);            		//爪子电机设置原点
        HAL_Delay(1);
		ClawR.pf->setOrigin(&ClawR);
        HAL_Delay(1);
		
        EscaladeL.pf->setOrigin(&EscaladeL);            //云梯电机设置原点
        HAL_Delay(1);
        EscaladeR.pf->setOrigin(&EscaladeR);
        HAL_Delay(1);
        ClawL.pf->setOrigin(&ClawL);            		//爪子电机设置原点
        HAL_Delay(1);
		ClawR.pf->setOrigin(&ClawR);
        HAL_Delay(5);
    }
	
    //云梯走到射球位置
	EscaladeL.pf->posMode(&EscaladeL, cEscaladeShootWaitAngleL, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
	HAL_Delay(1);
	EscaladeR.pf->posMode(&EscaladeR, cEscaladeShootWaitAngleR, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
	HAL_Delay(1);

	openClaw(HAL_Delay,1);						//打开爪子
	HAL_Delay(1);
#endif

	while ((ABS(EscaladeL.status.posation-cEscaladeShootWaitAngleL) > 5.f) || 
		(ABS(EscaladeR.status.posation-cEscaladeShootWaitAngleR) > 5.f))
	{											//等待云梯到达推射位置
		HAL_Delay(1);
	}
		
	while (1)                    				//等待推射触发限位开关
	{
		if (HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_4) == GPIO_PIN_SET)
		{
			Shoot.pf->dutyMode(&Shoot, 0);
			clutchBreak();
			HAL_Delay(100);
			clutchUnBreak();
			break;
		}
		Shoot.pf->dutyMode(&Shoot,-6.f);         //推射回到最底下(多次发防止timeOut)
		for (volatile uint16_t i; i<30000; i++)
		{
			//延时300us
		}
	}
	while(!shootEnc.pf->read(&shootEnc))
	{
		HAL_Delay(1);
	}
	
	gShootLastAngle = shootEnc.angle;
	gMechInitFinishAndStartEncFlag = true;
	HAL_Delay(100);
	
#if SHOOT_ENABLE
    if (gMechInitMode == 0xBB)
    {
        while(1)
        {
            if ((gShootNowAngle - cOneMinOriginAngle) > cDeathZone)
            {
                Shoot.pf->dutyMode(&Shoot, -10.f);
            }
            else if ((gShootNowAngle - cOneMinOriginAngle) < (-cDeathZone))
            {
                Shoot.pf->dutyMode(&Shoot, 10.f);
            }
            else
            {
                Shoot.pf->dutyMode(&Shoot, 0);
                clutchBreak();
                HAL_Delay(100);
                clutchUnBreak();
                break;
            }
            HAL_Delay(1);
        }
        EscaladeL.pf->posMode(&EscaladeL, cEscaladeButtonAngleL, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
        HAL_Delay(1);
        EscaladeR.pf->posMode(&EscaladeR, cEscaladeButtonAngleR, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
        HAL_Delay(1);

        closeClaw(HAL_Delay,1);				//合上爪子
        HAL_Delay(1);
    }
	
#endif

}


void myOsDelay(uint32_t myDelayTime)                    //封装后无返回值的osDelay
{
	osDelay(myDelayTime);
}

BaseType_t myQueueSend(QueueHandle_t queue, void *itemToSend, uint32_t delayTime)   //自动判断是在中断还是任务里的队列发送函数
{
	if(xPortIsInsideInterrupt())								// 判断是不是在中断中
	{
		BaseType_t sPDFALSE = pdFALSE;
		return xQueueSendFromISR(queue, itemToSend, &sPDFALSE);// 中断发送
	} else
	{
		return xQueueSend(queue, itemToSend, delayTime);		// 阻塞发送
	}
}

